#include "stm32f10x.h"  // Device header
#include "stdbool.h"
#include "Serial.h"

float line_folower(float kp, float ki, float kd)
{
    static float error = 0, last_error = 0, integral = 0;
	float output;
    bool s1 = GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_1);
    bool s2 = GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_2);
    bool s3 = GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_3);
    bool s4 = GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4);
	
	//printf("%d, %d, %d, %d\r\n", s1, s2, s3, s4);
	
    if( !s1 && s2 && !s3 && !s4) { /* 0 1 0 0 */
        error = -1;
    } else if(s1 && s2 && !s3 && !s4) { /* 1 1 0 0 */
        error = -2;
    } else if(s1 && !s2 && !s3 && !s4) { /* 1 0 0 0 */
        error = -6;
    } else if(!s1 && !s2 && s3 && !s4) { /* 0 0 1 0 */
        error = 1;
    } else if(!s1 && !s2 && s3 && s4) { /* 0 0 1 1 */
        error = 2;
    } else if(!s1 && !s2 && !s3 && s4) { /* 0 0 0 1 */
        error = 6;
    } else {
        error = 0;
    }
	integral += error;
	integral = integral > 80 ? 80 : (integral < -80 ? -80 : integral);
	 output = kp * error + ki * integral + kd * (error - last_error);
    return output;
}

